#!/usr/bin/python

import cv
import time
import pyglet
from pyglet.gl import *
import pyglet.image
import Image
import random
import sys
import math
from ConfigParser import *

class Video(object):
    def __init__(self):
        super(Video, self).__init__()
        self.LoadIniData('tracker.ini')

        self.storage = cv.CreateMemStorage(0)
        self.last = 0
        self.old_center = (0,0)

        self.capture = cv.CaptureFromCAM(self.cameraid)
        if not self.capture:
            print "Error opening capture device"
            sys.exit (1)

        self.frame = cv.QueryFrame (self.capture)
        self.size = cv.GetSize(self.frame)
        (self.width, self.height) = self.size
        self.motion = cv.CreateImage(self.size, cv.IPL_DEPTH_8U, 3)

        self.eye = cv.CreateImage(self.size, cv.IPL_DEPTH_8U, 3)
        self.mhi = cv.CreateImage(self.size, cv.IPL_DEPTH_32F, 1)
        self.orient = cv.CreateImage(self.size,cv.IPL_DEPTH_32F, 1)
        self.segmask = cv.CreateImage(self.size,cv.IPL_DEPTH_32F, 1)
        self.mask = cv.CreateImage(self.size,cv.IPL_DEPTH_8U, 1)
        self.temp = cv.CreateImage(self.size, cv.IPL_DEPTH_8U, 3)
        self.buf = range(10) 
        for i in range(self.n_frames):
            self.buf[i] = cv.CreateImage(self.size, cv.IPL_DEPTH_8U, 1)
            cv.SetZero(self.buf[i])
        
    def get_video(self):
        self.motion = cv.QueryFrame (self.capture)
        cv.Flip(self.motion, None, -1)
        video_image = pyglet.image.ImageData(self.width, self.height, 
                                             "BGR", self.motion.tostring()) 
        return video_image
    
    def process_motion(self):
        center = (-1, -1)
        # a lot of stuff from this section was taken from the code motempl.py, 
        #  openCV's python sample code
        timestamp = time.clock() / self.clocks_per_sec # get current time in seconds
        idx1 = self.last
        cv.CvtColor(self.motion, self.buf[self.last], cv.CV_BGR2GRAY) # convert frame to grayscale
        idx2 = (self.last + 1) % self.n_frames 
        self.last = idx2
        silh = self.buf[idx2]
        cv.AbsDiff(self.buf[idx1], self.buf[idx2], silh) # get difference between frames
        cv.Threshold(silh, silh, 30, 1, cv.CV_THRESH_BINARY) # and threshold it
        cv.UpdateMotionHistory(silh, self.mhi, timestamp, self.mhi_duration) # update MHI
        cv.ConvertScale(self.mhi, self.mask, 255./self.mhi_duration, 
                        (self.mhi_duration - timestamp)*255./self.mhi_duration)
        cv.SetZero(self.motion)
        cv.Merge(self.mask, None, None, None, self.motion)
        cv.CalcMotionGradient(self.mhi, self.mask, self.orient, self.max_time_delta, self.min_time_delta, 3)
        seq = cv.SegmentMotion(self.mhi, self.segmask, self.storage, timestamp, self.max_time_delta)
        inc = 0
        a_max = 0
        max_rect = -1
    
        # there are lots of things moving around
        #  in this case just find find the biggest change on the image
        for (area, value, comp_rect) in seq:
            if comp_rect[2] + comp_rect[3] > 60: # reject small changes
                if area > a_max: 
                    a_max = area
                    max_rect = inc
            inc += 1

        # found it, now just do some processing on the area.
        if max_rect != -1:
            (area, value, comp_rect) = seq[max_rect]
            color = cv.CV_RGB(255, 0,0)
            silh_roi = cv.GetSubRect(silh, comp_rect)
            # calculate number of points within silhouette ROI
            count = cv.Norm(silh_roi, None, cv.CV_L1, None)

            # this rectangle contains the overall motion ROI
            cv.Rectangle(self.motion, (comp_rect[0], comp_rect[1]), 
                         (comp_rect[0] + comp_rect[2], 
                          comp_rect[1] + comp_rect[3]), (0,0,255), 1)

            # the goal is to report back a center of movement contained in a rectangle
            # adjust the height based on the number generated by the slider bar
            h = int(comp_rect[1] + (comp_rect[3] * (float(self.height_value) / 100)))
            # then calculate the center
            center = ((comp_rect[0] + comp_rect[2] / 2), h)

            # and then slow it down.
            center = v_in.dejitter(center)

        return center

    def dejitter(self, center):
        # if the center was not found, just draw the center on the old location. 
        if center == (-1,-1):
            self.old_center = self.old_center
        else: 
            # anti-jittering section
            x1 = self.old_center[0]
            y1 = self.old_center[1]
            x2 = center[0] 
            y2 = center[1]
            dx = x2 - x1
            dy = y2 - y1
            d = math.sqrt( (dx)**2 + (dy)**2 ) # calculate a distance from old location to new.
            angle = math.atan2(dy, dx)  # get an angle while we're doing the math

            if d > self.jitter_value: # if the distance is really far....
                self.old_center = (x2, y2)
                # calculate a near point that exists on the line between the old and new point
                self.old_center = (cv.Round(x1 + self.jitter_value * math.cos(angle)),
                                   cv.Round(y1 + self.jitter_value * math.sin(angle)))
            else: # the distance is not far so just draw it
                self.old_center = (x2, y2)

        return(self.old_center)


    def LoadIniData(self, FileName):
        self.cp=ConfigParser()
        try:
            self.cp.readfp(open(FileName,'r'))
        except IOError:
            raise Exception,'NoFileError'

        self.store = self.cp.getboolean('Variables', 'store')
        self.cameraid = self.cp.getint('Variables', 'cameraid')
        self.FPS = self.cp.getint('Variables', 'fps')
        self.xwindows = self.cp.getint('Variables', 'xwindows')
        self.working_height = self.cp.getint('Variables', 'working_height')
        self.clocks_per_sec = self.cp.getfloat('Variables', 'clocks_per_sec')
        self.mhi_duration = self.cp.getfloat('Variables', 'mhi_duration')
        self.max_time_delta = self.cp.getfloat('Variables', 'max_time_delta')
        self.min_time_delta = self.cp.getfloat('Variables', 'min_time_delta')
        self.n_frames = self.cp.getint('Variables', 'n_frames')
        self.height_value = self.cp.getint('Variables', 'height_value')
        self.jitter_value = self.cp.getint('Variables', 'jitter_value')

        return


if __name__ == '__main__':
    cv.NamedWindow("Depth")

    v_in = Video()
    
    window = pyglet.window.Window()
    window.width = 1280
    window.height = 960

    def update_images(dt):
        global v_in, video, location
        video = v_in.get_video()
        location = v_in.process_motion()

    pyglet.clock.schedule_interval(update_images, 1/30.0)
        
    update_images(0.0)
    
    if video is not None:
        video_sprite = pyglet.sprite.Sprite(video)
        video_sprite.x = 0
        video_sprite.y = 0
        scale = window.width / v_in.width
        video_sprite.scale = scale

    side_length = 20
    vertices = [-side_length, side_length, 0, 0, side_length, side_length]
    vertices_gl = (GLfloat * len(vertices))(*vertices)

    glEnableClientState(GL_VERTEX_ARRAY)
    glVertexPointer(2, GL_FLOAT, 0, vertices_gl)

    @window.event
    def on_draw():
        window.clear()

        video_sprite.image = video
        video_sprite.draw()

        l = []
        for i in range(0,len(vertices)-1,2):
            l.append((vertices[i] + location[0]) * scale)
            l.append((vertices[i+1] + location[1]) * scale)

        l_gl = (GLfloat * len(l))(*l)
        glVertexPointer(2, GL_FLOAT, 0, l_gl)

        glDrawArrays(GL_LINE_LOOP, 0, len(l) // 2)

    pyglet.app.run()


